/*
 * Software for Interactive Project Pool Game
 * Columbia University. New York, 2008
 * Authors:
 *   Abdulhamid Ghandour
 *   Thomas John
 *   Jaime Peretzman
 *   Bharadwaj Vellore
 * 
 * Desc:
 */
#include <stdio.h>
#include "fixedpoint.h"
#include "gameconfig.h"
#include "types.h"
#include "debug.h"
#include "ball.h"

#define VEC_MAG_SQ(vec)         FPSUM(FPSQR(vec.x),FPSQR(vec.y))
#define VEC_DOT_PROD(vec1,vec2) FPSUM(                 \
  FPMUL(vec1.x,vec2.x),FPMUL(vec1.y,vec2.y)            \
)

long long ballRegisters[NUM_BALLS] = {
  BALL_0_BASE,
  BALL_1_BASE,
  BALL_2_BASE,
  BALL_3_BASE,
  BALL_4_BASE,
  BALL_5_BASE,
  BALL_6_BASE
};

#define DRAW_BALL(ballIndex,x,y,col)                     \
  IOWR_VAL(ballRegisters[ballIndex] + BALL_X,(x)),       \
  IOWR_VAL(ballRegisters[ballIndex] + BALL_Y,(y)),       \
  IOWR_VAL(ballRegisters[ballIndex] + BALL_COLOUR,(col))

#define VGA_NOT_READY   (IORD_16DIRECT(VGA_BASE, VGA_FLAG * 2) & 0x0001)
/*
// Ball dynamics implementation
*/
bool_t isBallMoving(const struct ball_t *ball){
  return (((ball->vel.x != 0) || (ball->vel.y != 0))? TRUE : FALSE);
}

long long collisionWithTableTime(const struct ball_t *ball, edge_t *edge){
  long long hColTime, vColTime;
  edge_t hozEdge, verEdge;
  hColTime = INT2FP(1000);
  vColTime = INT2FP(1000);

  DP_ASSERT(
    ((ball->vel.x != 0) || (ball->vel.y != 0)),
    "Collision check being performed for stationary ball"
  );
  
  if(ball->vel.x > 0){
    hColTime = FPDIV((TABLE_END_X - 
      (ball->pos.x + ball->radius)),ball->vel.x);
    hozEdge = RIGHT;
  }else if(ball->vel.x < 0){
    hColTime = FPDIV((TABLE_START_X - 
      (ball->pos.x - ball->radius)),ball->vel.x);
    hozEdge = LEFT;
  }else{
    /*
    // Ball is not moving along long this axis => Nothing to do
    */  
  }
  if(hColTime < 0) hColTime = INT2FP(1000);

  if(ball->vel.y > 0){
    vColTime = FPDIV((TABLE_END_Y - 
      (ball->pos.y + ball->radius)),ball->vel.y);
    verEdge = BOTTOM;
  }else if(ball->vel.y < 0){
    vColTime = FPDIV((TABLE_START_Y - 
      (ball->pos.y - ball->radius)),ball->vel.y);
    verEdge = TOP;
  }else{
    /*
    // Ball is not moving along long this axis => Nothing to do
    */  
  }
  if(vColTime < 0) vColTime = INT2FP(1000);

  if(hColTime > vColTime){
    *edge = verEdge;
    return vColTime;
  }else{
    *edge = hozEdge;
    return hColTime;
  }
}

long long collisionWithBallTime(
  const struct ball_t *ball1,
  const struct ball_t *ball2
){
  long long contactDist = ball1->radius + ball2->radius;
  struct vector relativeVelocity;
  struct vector relativeDisplacement;
  long long result;

  relativeVelocity.x = ball1->vel.x - ball2->vel.x;
  relativeVelocity.y = ball1->vel.y - ball2->vel.y;

  relativeDisplacement.x = ball1->pos.x - ball2->pos.x;
  relativeDisplacement.y = ball1->pos.y - ball2->pos.y;

  long long A = VEC_MAG_SQ(relativeVelocity);
  long long B = 2 * VEC_DOT_PROD(relativeDisplacement,relativeVelocity);
  long long C = VEC_MAG_SQ(relativeDisplacement) - FPSQR(contactDist);

  long long BSQ_MINUS_4AC = FPSQR(B) - 4LL * FPMUL(A,C);
  
  if((BSQ_MINUS_4AC < 0) || (A == 0)){
    result = INT2FP(1000LL); 
  }else{
    result = FPDIV((-B - FPSQRT(BSQ_MINUS_4AC)),(2LL * A));
  }

  return result;
}

void handleBallCollision(struct ball_t *ball1, struct ball_t *ball2){
  const long long A = FPDIV(INT2FP(22LL),INT2FP(30LL));
  const long long B = FPDIV(INT2FP(22LL),INT2FP(30LL));
  const long long C = FPDIV(INT2FP(22LL),INT2FP(30LL));
  const long long D = FPDIV(INT2FP(22LL),INT2FP(30LL));

  struct vector unitRelativeDisp1To2;
  struct vector tempUnitRelativeDisp1To2;

  unitRelativeDisp1To2.x = ball2->pos.x - ball1->pos.x;
  unitRelativeDisp1To2.y = ball2->pos.y - ball1->pos.y;

  long long relativeDispMag = FPSQRT(VEC_MAG_SQ(unitRelativeDisp1To2));
  unitRelativeDisp1To2.x = 
    FPDIV(unitRelativeDisp1To2.x,relativeDispMag);
  unitRelativeDisp1To2.y = 
    FPDIV(unitRelativeDisp1To2.y,relativeDispMag);

  tempUnitRelativeDisp1To2.x = unitRelativeDisp1To2.x;
  tempUnitRelativeDisp1To2.y = unitRelativeDisp1To2.y;

  long long ball1VelocityComp = 
    VEC_DOT_PROD(ball1->vel,unitRelativeDisp1To2);
  long long ball2VelocityComp = 
    VEC_DOT_PROD(ball2->vel,unitRelativeDisp1To2);

  long long newVelocityCompMagBall1 = 
    FPMUL(A,ball1VelocityComp) - FPMUL(B,ball2VelocityComp);
  long long newVelocityCompMagBall2 = 
    FPMUL(C,ball2VelocityComp) - FPMUL(D,ball1VelocityComp);

  unitRelativeDisp1To2.x = FPMUL(
    (unitRelativeDisp1To2.x),newVelocityCompMagBall1
  );
  unitRelativeDisp1To2.y = FPMUL(
    (unitRelativeDisp1To2.y),newVelocityCompMagBall1
  );

  tempUnitRelativeDisp1To2.x = FPMUL(
    (tempUnitRelativeDisp1To2.x),newVelocityCompMagBall2
  );
  tempUnitRelativeDisp1To2.y = FPMUL(
    (tempUnitRelativeDisp1To2.y),newVelocityCompMagBall2
  );
  
  ball1->vel.x -= unitRelativeDisp1To2.x;
  ball1->vel.y -= unitRelativeDisp1To2.y;
  ball2->vel.x -= tempUnitRelativeDisp1To2.x;
  ball2->vel.y -= tempUnitRelativeDisp1To2.y;
}

long long handleCollisionWithCue(
  struct ball_t *ball1,
  const struct ball_t *cue
){
  const long long A = FPDIV(INT2FP(8LL),INT2FP(50LL));
  const long long B = FPDIV(INT2FP(8LL),INT2FP(50LL));

  struct vector unitRelativeDisp1To2;
  struct vector tempUnitRelativeDisp1To2;

  unitRelativeDisp1To2.x = cue->pos.x - ball1->pos.x;
  unitRelativeDisp1To2.y = cue->pos.y - ball1->pos.y;

  long long relativeDispMag = FPSQRT(VEC_MAG_SQ(unitRelativeDisp1To2));
  unitRelativeDisp1To2.x = 
    FPDIV(unitRelativeDisp1To2.x,relativeDispMag);
  unitRelativeDisp1To2.y = 
    FPDIV(unitRelativeDisp1To2.y,relativeDispMag);

  tempUnitRelativeDisp1To2.x = unitRelativeDisp1To2.x;
  tempUnitRelativeDisp1To2.y = unitRelativeDisp1To2.y;

  long long ball1VelocityComp = 
    VEC_DOT_PROD(ball1->vel,unitRelativeDisp1To2);
  long long cueVelocityComp = 
    VEC_DOT_PROD(cue->vel,unitRelativeDisp1To2);

  long long newVelocityCompMagBall1 = 
    FPMUL(A,ball1VelocityComp) - FPMUL(B,cueVelocityComp);

  unitRelativeDisp1To2.x = FPMUL(
    (unitRelativeDisp1To2.x),newVelocityCompMagBall1
  );
  unitRelativeDisp1To2.y = FPMUL(
    (unitRelativeDisp1To2.y),newVelocityCompMagBall1
  );

  ball1->vel.x -= unitRelativeDisp1To2.x;
  ball1->vel.y -= unitRelativeDisp1To2.y;

  return 0;
}

void moveBalls(struct ball_t *balls, long long time){
  int i;

  for(i=0; i< NUM_BALLS; i++){
    if((BALL_INVISIBLE != balls[i].ballState) && 
       (TRUE == isBallMoving(&balls[i]))
    ){
      balls[i].pos.x += FPMUL(balls[i].vel.x,time);
      balls[i].pos.y += FPMUL(balls[i].vel.y,time);
    }
  }
}

void drawBalls(struct ball_t *balls){
  int i;

  while(VGA_NOT_READY == 1);
  
  for(i=0; i<NUM_BALLS; i++){
    if(BALL_INVISIBLE == balls[i].ballState){
      balls[i].colour = COL_INVISIBLE;
    }
    /*
    // Tell the hardware to draw the balls on screen
    */
    //DP2("Drawing Ball at (%lld, %lld)\n",SPRITE_X(balls[i]),SPRITE_Y(balls[i]));
    DRAW_BALL(i,SPRITE_X(balls[i]),SPRITE_Y(balls[i]),balls[i].colour);
  }
}

void applyFriction(struct ball_t *balls){
  int i;
  long long newVelX, newVelY;

  for(i=0; i<NUM_BALLS; i++){
    if(BALL_INVISIBLE == balls[i].ballState){
      continue;
    }

    newVelX = balls[i].vel.x - FPMUL(balls[i].vel.x,DAMPING_COEFF);
    if(FPMUL(newVelX, balls[i].vel.x) > 0){
      balls[i].vel.x = newVelX;
    }
    else{
      balls[i].vel.x = 0;
    }

    newVelY = balls[i].vel.y - FPMUL(balls[i].vel.y,DAMPING_COEFF);
    if(FPMUL(newVelY, balls[i].vel.y) > 0){
      balls[i].vel.y = newVelY;
    }
    else{
      balls[i].vel.y = 0;
    }
  }     
}
